/*
 *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *  2. Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *  3. Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  controller_action.cpp
 *
 *  authors:
 *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
 *    Jorge Santos Simón <santos@magazino.eu>
 *
 */

#include "mbf_abstract_nav/controller_action.h"

namespace mbf_abstract_nav
{

ControllerAction::ControllerAction(const std::string &action_name, const mbf_utility::RobotInformation &robot_info):
        AbstractActionBase(action_name, robot_info) {}

void ControllerAction::start(GoalHandle &goal_handle, typename AbstractControllerExecution::Ptr execution_ptr)
{
    if (goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING)
    {
        goal_handle.setCanceled();
        return;
    }
    uint8_t slot = goal_handle.getGoal()->concurrency_slot;
    bool update_plan = false;
    slot_map_mtx_.lock();
    auto slot_it = concurrency_slots_.find(slot);
    if (slot_it != concurrency_slots_.end() && slot_it->second.in_use)
    {
        boost::lock_guard<boost::mutex> goal_guard(goal_mtx_);
        if (slot_it->second.execution->getName() == goal_handle.getGoal()->controller ||
            goal_handle.getGoal()->controller.empty())
        {
            update_plan = true;
            // Goal requests to run the same controller on the same concurrency slot already in use:
            // we update the goal handle and pass the new plan and tolerances from the action to the
            // execution without stopping it
            execution_ptr = slot_it->second.execution;
            execution_ptr->setNewPlan(goal_handle.getGoal()->path.poses,
                                      goal_handle.getGoal()->tolerance_from_action,
                                      goal_handle.getGoal()->dist_tolerance,
                                      goal_handle.getGoal()->angle_tolerance);
            // Update also goal pose, so the feedback remains consistent
            goal_pose_ = goal_handle.getGoal()->path.poses.back();
            mbf_msgs::ExePathResult result;
            fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result);
            concurrency_slots_[slot].goal_handle.setCanceled(result, result.message);
            concurrency_slots_[slot].goal_handle = goal_handle;
            concurrency_slots_[slot].goal_handle.setAccepted();
        }
    }
    slot_map_mtx_.unlock();
    // Otherwise run parent version of this method
    if (!update_plan) AbstractActionBase::start(goal_handle, execution_ptr);
}

void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecution &execution)
{
    goal_mtx_.lock();
    // Note that we always use the goal handle stored on the concurrency slots map, as it can change when replanning
    uint8_t slot = goal_handle.getGoal()->concurrency_slot;
    goal_mtx_.unlock();
    ROS_DEBUG_STREAM_NAMED(name_, "Start action " << name_);
    // ensure we don't provide values from previous execution on case of error before filling both poses
    goal_pose_ = geometry_msgs::PoseStamped();
    robot_pose_ = geometry_msgs::PoseStamped();
    ros::NodeHandle private_nh("~");
    double oscillation_timeout_tmp;
    private_nh.param("oscillation_timeout", oscillation_timeout_tmp, 0.0);
    ros::Duration oscillation_timeout(oscillation_timeout_tmp);
    double oscillation_distance;
    private_nh.param("oscillation_distance", oscillation_distance, 0.03);
    mbf_msgs::ExePathResult result;
    mbf_msgs::ExePathFeedback feedback;
    typename AbstractControllerExecution::ControllerState state_moving_input;
    bool controller_active = true;
    goal_mtx_.lock();
    const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
    const std::vector<geometry_msgs::PoseStamped> &plan = goal.path.poses;
    if (plan.empty())
    {
        fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan!", result);
        goal_handle.setAborted(result, result.message);
        ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
        controller_active = false;
        goal_mtx_.unlock();
        return;
    }
    goal_pose_ = plan.back();
    ROS_DEBUG_STREAM_NAMED(name_, "Called action \""
            << name_ << "\" with plan:" << std::endl
            << "frame: \"" << goal.path.header.frame_id << "\" " << std::endl
            << "stamp: " << goal.path.header.stamp << std::endl
            << "poses: " << goal.path.poses.size() << std::endl
            << "goal: (" << goal_pose_.pose.position.x << ", "
            << goal_pose_.pose.position.y << ", "
            << goal_pose_.pose.position.z << ")");
    goal_mtx_.unlock();
    geometry_msgs::PoseStamped oscillation_pose;
    ros::Time last_oscillation_reset = ros::Time::now();
    bool first_cycle = true;
    while (controller_active && ros::ok())
    {
        // goal_handle could change between the loop cycles due to adapting the plan
        // with a new goal received for the same concurrency slot
        if (!robot_info_.getRobotPose(robot_pose_))
        {
            controller_active = false;
            fillExePathResult(mbf_msgs::ExePathResult::TF_ERROR, "Could not get the robot pose!", result);
            goal_mtx_.lock();
            goal_handle.setAborted(result, result.message);
            goal_mtx_.unlock();
            ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
            break;
        }
        // init oscillation pose
        if (first_cycle) oscillation_pose = robot_pose_;
        goal_mtx_.lock();
        state_moving_input = execution.getState();
        switch (state_moving_input)
        {
            case AbstractControllerExecution::INITIALIZED:
                execution.setNewPlan(plan, goal.tolerance_from_action, goal.dist_tolerance, goal.angle_tolerance);
                execution.start();
                break;

            case AbstractControllerExecution::STOPPED:
                ROS_WARN_STREAM_NAMED(name_, "The controller has been stopped rigorously!");
                controller_active = false;
                result.outcome = mbf_msgs::ExePathResult::STOPPED;
                result.message = "Controller has been stopped!";
                goal_handle.setAborted(result, result.message);
                break;

            case AbstractControllerExecution::CANCELED:
                ROS_INFO_STREAM("Action \"exe_path\" canceled");
                fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Controller canceled", result);
                goal_handle.setCanceled(result, result.message);
                controller_active = false;
                break;

            case AbstractControllerExecution::STARTED:
                ROS_DEBUG_STREAM_NAMED(name_, "The moving has been started!");
                break;

            case AbstractControllerExecution::PLANNING:
                if (execution.isPatienceExceeded())
                {
                    ROS_INFO_STREAM("Try to cancel the plugin \"" << name_ << "\" after the patience time has been exceeded!");
                    if (execution.cancel())
                    {
                        ROS_INFO_STREAM("Successfully canceled the plugin \"" << name_ <<
                                        "\" after the patience time has been exceeded!");
                    }
                }
                break;

            case AbstractControllerExecution::MAX_RETRIES:
                ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!");
                controller_active = false;
                fillExePathResult(execution.getOutcome(), execution.getMessage(), result);
                goal_handle.setAborted(result, result.message);
                break;

            case AbstractControllerExecution::PAT_EXCEEDED:
                ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the patience time");
                controller_active = false;
                fillExePathResult(mbf_msgs::ExePathResult::PAT_EXCEEDED, execution.getMessage(), result);
                goal_handle.setAborted(result, result.message);
                break;

            case AbstractControllerExecution::NO_PLAN:
                ROS_WARN_STREAM_NAMED(name_, "The controller has been started without a plan!");
                controller_active = false;
                fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started without a path", result);
                goal_handle.setAborted(result, result.message);
                break;

            case AbstractControllerExecution::EMPTY_PLAN:
                ROS_WARN_STREAM_NAMED(name_, "The controller has received an empty plan");
                controller_active = false;
                fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan", result);
                goal_handle.setAborted(result, result.message);
                break;

            case AbstractControllerExecution::INVALID_PLAN:
                ROS_WARN_STREAM_NAMED(name_, "The controller has received an invalid plan");
                controller_active = false;
                fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an invalid plan", result);
                goal_handle.setAborted(result, result.message);
                break;

            case AbstractControllerExecution::NO_LOCAL_CMD:
                ROS_WARN_STREAM_THROTTLE_NAMED(3, name_, "No velocity command received from controller! "
                        << execution.getMessage());
                controller_active = execution.isMoving();
                if (!controller_active)
                {
                    fillExePathResult(execution.getOutcome(), execution.getMessage(), result);
                    goal_handle.setAborted(result, result.message);
                }
                else
                {
                    publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(),
                                           execution.getVelocityCmd());
                }
                break;

            case AbstractControllerExecution::GOT_LOCAL_CMD:
                if (!oscillation_timeout.isZero())
                {
                    // check if oscillating
                    if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance)
                    {
                        last_oscillation_reset = ros::Time::now();
                        oscillation_pose = robot_pose_;
                    }
                    else if (last_oscillation_reset + oscillation_timeout < ros::Time::now())
                    {
                        ROS_WARN_STREAM_NAMED(name_, "The controller is oscillating for "
                                << (ros::Time::now() - last_oscillation_reset).toSec() << "s");
                        execution.cancel();
                        controller_active = false;
                        fillExePathResult(mbf_msgs::ExePathResult::OSCILLATION, "Oscillation detected!", result);
                        goal_handle.setAborted(result, result.message);
                        break;
                    }
                }
                publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(),
                                       execution.getVelocityCmd());
                break;

            case AbstractControllerExecution::ARRIVED_GOAL:
                ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal");
                controller_active = false;
                fillExePathResult(mbf_msgs::ExePathResult::SUCCESS, "Controller succeeded; arrived at goal!", result);
                goal_handle.setSucceeded(result, result.message);
                break;

            case AbstractControllerExecution::INTERNAL_ERROR:
                ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin: "
                        << execution.getMessage());
                controller_active = false;
                fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR,
                                  "Internal error: Unknown error thrown by the plugin!", result);
                goal_handle.setAborted(result, result.message);
                break;

            default:
                std::stringstream ss;
                ss << "Internal error: Unknown state in a move base flex controller execution with the number: "
                   << static_cast<int>(state_moving_input);
                fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, ss.str(), result);
                ROS_FATAL_STREAM_NAMED(name_, result.message);
                goal_handle.setAborted(result, result.message);
                controller_active = false;
        }
        goal_mtx_.unlock();
        if (controller_active)
        {
            // try to sleep a bit
            // normally this thread should be woken up from the controller execution thread
            // in order to transfer the results to the controller
            execution.waitForStateUpdate(boost::chrono::milliseconds(500));
        }
        first_cycle = false;
    }  // while (controller_active && ros::ok())
    if (!controller_active) ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
        // normal on continuous replanning
    else ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
}

void ControllerAction::publishExePathFeedback(
        GoalHandle &goal_handle,
        uint32_t outcome, const std::string &message,
        const geometry_msgs::TwistStamped &current_twist)
{
    mbf_msgs::ExePathFeedback feedback;
    feedback.outcome = outcome;
    feedback.message = message;
    feedback.last_cmd_vel = current_twist;
    if (feedback.last_cmd_vel.header.stamp.isZero())
        feedback.last_cmd_vel.header.stamp = ros::Time::now();
    feedback.current_pose = robot_pose_;
    feedback.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
    feedback.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
    goal_handle.publishFeedback(feedback);
}

void ControllerAction::fillExePathResult(
        uint32_t outcome, const std::string &message,
        mbf_msgs::ExePathResult &result)
{
    result.outcome = outcome;
    result.message = message;
    result.final_pose = robot_pose_;
    result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
    result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
}

} /* mbf_abstract_nav */
